/*
  Program Name:  MC_Main
 Project:       LLH Hopscotch
 Written By:    Daniel Lee
 Revision:      0.7.PS2
 Change log:   
 0.6.1...    Adding console debugging
 Adding code to send manual commands from the serial console
 for test the various functions of the network.
 0.6.3...    Interruptible Auto-mode is now functional
 0.7.0...    Implementing "Manual" and "Program" modes
 */

#include<SoftwareSerial.h>

#include <PS2X_lib.h>//include library for PS2

// Pins 2 and 7 are reserved for FTDI comms
// Pins 0 and 1 are reserved for USB comms (during programming only)

//PS2 PINS
#define PS2_DAT        13  //14 BROWN  *this right here needs a 10k pull up resistor to a 5v rail.(PS2 outputs digital 3.3 and arduino wants 5)
#define PS2_CMD        11  //15 ORANGE
#define PS2_SEL        10  //16 YELLOW
#define PS2_CLK        12  //17 BLUE
//Pull GREEN up with 10k resistor, connect to 5v OUT on arduino
//WHITE is unknown
//RED is power 3.3v, connect to 3.3v OUT on arduino
//GREY is power for vibration is 7.6V *not working with my controller... maybe broken?
//BLACK is ground. duh.
#define pressures   false
#define rumble      false
//we don't need rumble or pressure
PS2X ps2x; // create PS2 Controller Class
int error = 0;
byte type = 0;
byte vibrate = 0;
//more PS2 Stuff

#define TRXRATE 19200
#define TERMBAUD 9600
#define LEDPIN 3
#define SENSORPIN A5
#define SPKRPIN 11
#define SEEDPIN A1
#define RX_boards 19
#define TX_boards 18
#define MAX_BUFFER_SIZE 64
#define MAX_NET_LEN 16
#define TIME_OUT 25
#define MSG_RCVD 102
#define ID_REQUEST 103
#define SETUP_ID 104
#define SENDING 0
#define READING 1
#define COMPLETE 2
#define CONFIRM_IDSET 3

//COMMAND CODES:
#define COM 170
#define ROGER 171
#define NEGATIVE 172
#define SETID 173
#define SENDID 174
#define SETNEIGHBORS 175
#define IDSET 176
#define NEIGHBORSSET 177
#define STATE_CHANGE 178
#define CURSOR 179
#define AMCURSOR 180
#define SEND_CURSOR 181
#define STEP_ON_ME 182 //WHERE DOES THIS APPEAR?
#define MANUAL 183 //PS2
#define PROGRAM 184
#define AUTO 200 //PS2
#define NOCOM 255

//SoftwareSerial board_port(RX_1,TX_1);

int active_channel;
int listen_count;
int state;
long prevTime;
int ledState;
uint8_t myID;
uint8_t cursor_ID;
uint8_t board_ID;
uint8_t read_buf[MAX_BUFFER_SIZE];
uint8_t write_buf[MAX_BUFFER_SIZE];
int read_buf_len;
int write_buf_len;
int send_status;
int next_available_ID;
int net_map[16][5];
int net_map_len;
uint8_t con_buf[MAX_BUFFER_SIZE];
int con_buf_len;
int target_ID; //Was causing cross init errors in switch statement, had to move it.


void blink_led(int ledPin){
  long blinkTime = 250;
  long curTime = millis();
  if((curTime - prevTime) > blinkTime){
    prevTime = curTime;
    if(ledState == LOW){
      ledState = HIGH;
    }
    else{
      ledState = LOW;
    }
  }
  digitalWrite(ledPin, ledState);
}

void writebuffertoboards(uint8_t *buf, int buf_len){
  const uint8_t start_sig = COM;
  int temp_len = buf_len;
  uint8_t temp_buf[buf_len+1];
  while(temp_len > 0){
    temp_buf[temp_len] = buf[temp_len-1];
    temp_len--;
  }
  temp_buf[0] = start_sig;
  Serial.print("Writing");
  for(int i=0;i<buf_len+1;i++){
    Serial.print(", ");
    Serial.print(temp_buf[i], DEC);
  }
  Serial.println(" to serial the board network.");
  Serial1.write(temp_buf, buf_len+1);
}

void read_from_boards(){
  if(Serial1.read() == COM){
    int cnt=0;
    uint8_t cur_read = -1;
    while(Serial1.available() > 0 && cnt < MAX_BUFFER_SIZE && cur_read != COM){
      cur_read = Serial1.read();
      read_buf[cnt] = cur_read;
      cnt++;
    }
    read_buf_len = cnt;
    send_msg_received();
  }
  else{
    Serial1.flush();
  }
}

int send_msg(){
  int count = 0;
  int confirm_rcvd = 0;
  int temp_buf_len = 0;
  uint8_t temp_buf[MAX_BUFFER_SIZE];
  while(confirm_rcvd != 1 && count < TIME_OUT){
    writebuffertoboards(write_buf, write_buf_len);
    delay(30);
    if(Serial1.read() == COM){
      int cnt = 0;
      while(Serial1.available() > 0 && Serial1.peek() != COM){
        temp_buf[cnt] = Serial1.read();
        cnt++;
      }
      temp_buf_len = cnt;
      if(cnt > 0 && temp_buf[cnt-1] == ROGER){
        confirm_rcvd = 1;
        Serial.println("...");
      }
      else{
        while(Serial1.available() > 0)Serial1.read();
      }
    }
    else{
      //if the input buffer doesn't start with '170', flush the garbage.
      read_buf_len = 0;
      while(Serial1.available() > 0)Serial1.read();
    }
    count++;
  }
  delay(60);
  while(Serial1.available() > 0)Serial1.read();
  read_buf_len = 0;

  if(count >= TIME_OUT || temp_buf_len <= 0){
    return -1;
  }
  else{
    return 0;
  }
}

void send_msg_received(){
  uint8_t msg_rcvd = ROGER;
  for(int i=0;i<2;i++){
    writebuffertoboards(&msg_rcvd, 1);
    delayMicroseconds(500);
  }
  //returning with the assumption the our confirmation was received
}


int find_next_setup_ID(){
  //Set up IDs (id selection):
  int temp_row = net_map_len - 1;
  if (temp_row < 0){
    return 1;
  }
  else{
    //check the current row for an ID larger than the row's initial ID
    int test_ID = 0;
    while(test_ID == 0 && temp_row >= 0){
      int row_ID = net_map[temp_row][0];
      int i=0;
      int ID_found = 0;
      while(ID_found == 0 && i < 4){
        if(net_map[temp_row][i+1] < 255 && net_map[temp_row][i+1] > row_ID){
          int temp_ID = net_map[temp_row][i+1];
          //now check to make sure temp_ID doesn't already have an entry in the net_map
          int entry_exists = 0;
          for(int j=0;j<net_map_len;j++){
            if(net_map[j][0] == temp_ID){
              //ID already has a map entry
              entry_exists = 1;
            }
          }
          if(entry_exists == 0){
            ID_found = 1;
            test_ID = temp_ID;
          }
        }
        i++;
      }
      temp_row--;
    }
    return test_ID;
  }
}

void prep_remote_msg(int target_ID){
  int path_ID = target_ID;
  write_buf_len = 0;
  if(target_ID > 1){
    Serial.print("Adding ");
    Serial.print(target_ID);
    Serial.println(" to write_buf");
    write_buf[0] = target_ID;
    write_buf_len = 1;
    //look for target ID among the ports (columns 1 thru 4) of the net_map array
    int j=0;
    int k=0;
    int done = 0;
    int found_ID = 0;
    int dir = 1;
    while(done == 0){
      for(k=0;k<4;k++){
        Serial.println(net_map[j][k+1]);
        if(net_map[j][k+1] == path_ID){
          dir = -1;
          found_ID = 1;
        }
      }
      j += dir;
      if(found_ID == 1){
        int index = j+1;
        //ID was found at net_map[index][0]
        path_ID = net_map[index][0];
        Serial.print("found ID: ");
        Serial.print(path_ID);
        Serial.print(" at index: ");
        Serial.println(index);
        if(path_ID > 1){
          //shift it in to the write buffer
          int cnt = write_buf_len;  //at this point write_buf_len is at least  '1'
          while(cnt > 0){
            write_buf[cnt] = write_buf[cnt-1];
            cnt--;
          }
          Serial.print("Adding ");
          Serial.print(path_ID);
          Serial.println(" to write_buf");
          write_buf[cnt] = path_ID;
          write_buf_len++;
        }
        else{
          done = 1;
        }
        found_ID = 0;
      }
    }
  }
  Serial.print("write_buf_len: ");
  Serial.println(write_buf_len);
  Serial.println("write_buf:");
  for(int q=0;q<write_buf_len;q++){
    Serial.print(write_buf[q]);
  }
  Serial.println("");
}

//only for use with the setneighbors command
void append_return_path(int target_ID){
  //write_buf[write_buf_len-3] should be the target ID
  int cnt = write_buf_len-4;
  while(cnt >= 0){
    write_buf[write_buf_len] = write_buf[cnt];
    cnt--;
    write_buf_len++;
  }
  if(target_ID > 1){
    write_buf[write_buf_len] = 1;
    write_buf_len++;
  }
  write_buf[write_buf_len] = 0;
  write_buf_len++;
}

void setup(){

  //PS2 debugging and setup
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);

  if(error == 0){
    Serial.print("Found Controller, configured successful ");
    Serial.print("pressures = ");
    if (pressures)
      Serial.println("true ");
    else
      Serial.println("false");
    Serial.print("rumble = ");
    if (rumble)
      Serial.println("true)");
    else
      Serial.println("false");
    Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
    Serial.println("holding L1 or R1 will print out the analog stick values.");
    Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
  }  
  else if(error == 1)
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");

  else if(error == 2)
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

  else if(error == 3)
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");

  type = ps2x.readType(); 
  switch(type) {
  case 0:
    Serial.print("Unknown Controller type found ");
    break;
  case 1:
    Serial.print("DualShock Controller found ");
    break;
  case 2:
    Serial.print("GuitarHero Controller found ");
    break;
  case 3:
    Serial.print("Wireless Sony DualShock Controller found ");
    break;
  }




  Serial.begin(TERMBAUD);
  Serial.println("attempting to initialize Serial comms...");
  Serial.println("...");
  Serial1.begin(TRXRATE);
  pinMode(LEDPIN, OUTPUT);
  Serial.println("...initialization complete.");
  randomSeed(analogRead(SEEDPIN));
  myID = 0;
  cursor_ID = 0;
  board_ID = -1;
  active_channel = 1;
  listen_count = 1;
  digitalWrite(LEDPIN, HIGH);
  state = SENDING;
  read_buf_len = 0;
  write_buf_len = 0;
  for(int j=0;j<16;j++){
    for(int k=0;k<5;k++){
      net_map[j][k] = -1;
    }
  }
  net_map_len = 0;
  ledState = LOW;
  next_available_ID = 1;
  prevTime = millis();
  //send_status = -1;

  type = ps2x.readType(); //debuggin for PS2
  switch(type) {
  case 0:
    Serial.print("Unknown Controller type found ");
    break;
  case 1:
    Serial.print("DualShock Controller found ");
    break;
  case 2:
    Serial.print("GuitarHero Controller found ");
    break;
  case 3:
    Serial.print("Wireless Sony DualShock Controller found ");
    break;
  }


}

void loop(){

  if(type == 0) { //DualShock Controller
    ps2x.read_gamepad(); //read controller and set large motor to spin at 'vibrate' speed

      if(ps2x.Button(PSB_START)){      //will be TRUE as long as button is pressed
      Serial.println("Start is being held");
      uint8_t make_auto = AUTO;
      writebuffertoboards(&make_auto, 3);
    }
    else 
      Serial.print("Start is not being held");
  }


  switch(state){
  case SENDING:
    write_buf[0] = SENDID;
    write_buf_len = 1;
    send_status = send_msg();
    if(send_status != 0){
      //something went wrong with board to board comms
      Serial.print("Error: status code = ");
      Serial.print(send_status);
      Serial.println(". Something went wrong with board-to-board comms!!!");
    }
    else{
      state = READING;
    }
    break;

  case READING:
    delay(1);
    if(Serial1.read() == COM){
      Serial.println("COM");
      //uint8_t cur_read = -1;
      int cnt = 0;
      while(Serial1.available() > 0 && cnt < MAX_BUFFER_SIZE && (uint8_t)Serial1.peek() != COM){
        //cur_read = Serial1.read();
        read_buf[cnt] = Serial1.read(); //cur_read;
        cnt++;
        Serial.println("available");
        delay(1);
      }
      read_buf_len = cnt;
    }
    if(read_buf_len > 0){
      send_msg_received();
      Serial.println("Received message from boards:");
      for(int i=0;i<read_buf_len;i++){
        Serial.print(read_buf[i]);
      }
      Serial.println("");
      switch(read_buf[0]){
      case IDSET:
        board_ID = read_buf[1];
        next_available_ID = board_ID + 1;
        state = SETNEIGHBORS;
        break;
      case NEIGHBORSSET:
        if(read_buf_len == 7){
          for(int i=0;i<5;i++){
            net_map[net_map_len][i] = read_buf[i+1];
          }
          net_map_len++;
          next_available_ID = read_buf[6];
          state = SETNEIGHBORS;
        }
        else{
          //we didn't get the whole message.
          Serial.print("Read Buffer Length = ");
          Serial.println(read_buf_len);
        }
        break;
      case SENDID:
        send_status = -1;
        write_buf[0] = myID;
        write_buf_len = 1;
        send_status = send_msg();
        if(send_status != 0){
          //something went wrong with board to board comms
          Serial.print("Error: status code = ");
          Serial.print(send_status);
          Serial.println(". Something went wrong with board-to-board comms!!!");
        }
        else{
          state = READING;
        }
        break;
      case AMCURSOR:
        cursor_ID = read_buf[1];
        state = COMPLETE;
        break;
      default:
        board_ID = read_buf[0];
        state = SETID;
        break;
      }
    }
    //while(Serial1.available() > 0)Serial1.read();
    //Serial.println("%%%");
    break;

  case COMPLETE:
    //Serial.println("Auto signal sent!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is: ");
    Serial.println(board_ID);
    Serial.println("System map:");
    for(int j=0;j<net_map_len;j++){
      for(int k=0;k<5;k++){
        Serial.print(net_map[j][k]);
        Serial.print(", ");
      }
      Serial.println("");
    }   
    Serial.println("***** SETUP COMPLETE *****");




    //Check for input from the console
    con_buf_len = 0;
    while(Serial.available() > 0){
      con_buf[con_buf_len] = Serial.parseInt();
      con_buf_len++;
    }
    //if there was a message from the console it will be
    //stored in con_buf
    if(con_buf_len > 0){
      if(con_buf[0] == STATE_CHANGE){
        state = con_buf[1];
      }
      else{
        for(int i=0;i<con_buf_len;i++){
          write_buf[i] = con_buf[i];
        }
        write_buf_len = con_buf_len;
        send_msg();
      }
    }





    delay(9000);
    break;

  case SETID:
    Serial.println("ID request successful!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is: ");
    Serial.println(board_ID);
    Serial.println("***** TEST COMPLETE *****");
    write_buf[0] = SETID;
    write_buf[1] = next_available_ID;
    write_buf_len = 2;
    send_status = send_msg();
    if(send_status != 0){
      Serial.print("Error: status code = ");
      Serial.print(send_status);
      Serial.println(". Something went wrong with board-to-board comms!!!");
    }
    else{
      state = READING;
    }
    break;

  case SETNEIGHBORS:
    target_ID = 0;
    Serial.println("Set ID successful!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is now: ");
    Serial.println(board_ID);
    Serial.println("***** TEST COMPLETE *****");
    target_ID = find_next_setup_ID();
    if(target_ID == 0){
      state = COMPLETE;
    }
    else{
      prep_remote_msg(target_ID);
      write_buf[write_buf_len] = SETNEIGHBORS;
      write_buf[write_buf_len+1] = next_available_ID;
      write_buf_len = write_buf_len+2;
      append_return_path(target_ID);
      send_status = send_msg();
      if(send_status != 0){
        Serial.print("Error: status code = ");
        Serial.print(send_status);
        Serial.println(". Something went wrong with board-to-board comms!!!");
      }
      else{
        state = READING;
      }
    }
    break;

  case AUTO:

    Serial.println("Set Neighbors Successful!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is: ");
    Serial.println(board_ID);
    Serial.println("System map:");
    for(int j=0;j<net_map_len;j++){
      for(int k=0;k<5;k++){
        Serial.print(net_map[j][k]);
        Serial.print(", ");
      }
      Serial.println("");
    }
    Serial.println("***** TEST COMPLETE *****");
    Serial.println("Beginning autonomous test!");
    //select the starting board from all available boards in the net_map
    //start by creating an array of all known IDs
    int start_ID;
    int list_of_IDs[MAX_NET_LEN];
    int list_len;
    int rand_index;
    for(list_len=0;list_len<net_map_len;list_len++){
      list_of_IDs[list_len] = net_map[list_len][0];
    }
    //choose an index at random
    rand_index = (int)random(list_len);
    start_ID = list_of_IDs[rand_index];
    //send the AUTO command to the chosen ID
    prep_remote_msg(start_ID);
    write_buf[write_buf_len] = AUTO;
    write_buf_len++;
    Serial.print("Sending autonomous mode signal to ID = ");
    Serial.println(start_ID);
    send_msg();
    //Once the message is sent, no response from the network is currently being expected
    //this may change in future versions
    state = COMPLETE;

    break;
  case MANUAL:
    write_buf[0] = CURSOR;
    write_buf_len = 1;
    send_status = send_msg();
    if(send_status != 0){
      Serial.print("Error: status code = ");
      Serial.print(send_status);
      Serial.println(". Something went wrong with board-to-board comms!!!");
    }
    else{
      state = READING;
    }
    break;
  case PROGRAM:
    break;
  default:
    break;
  }
}

